diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4b251642ce..c2c6e3d7bb 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -25,8 +25,7 @@ if [ -d NuttX/nuttx ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -47,8 +46,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -70,8 +68,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d src/lib/eigen ] @@ -92,8 +89,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/gencpp ] @@ -114,8 +110,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/genmsg ] @@ -136,8 +131,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi exit 0 diff --git a/src/lib/uavcan b/src/lib/uavcan index 1f1679c75d..46793c5e06 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 1f1679c75d989420350e69339d48b53203c5e874 +Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46 diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index c6719d4817..51ee776a97 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -68,8 +68,8 @@ int UavcanEscController::init() { // ESC status subscription int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("ESC status sub failed %i", res); return res; } @@ -83,9 +83,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || - (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || - (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } @@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Rate limiting - we don't want to congest the bus */ const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { return; } + _prev_cmd_pub = timestamp; /* @@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - // trim negative values back to 0. Previously - // we set this to 0.1, which meant motors kept - // spinning when armed, but that should be a - // policy decision for a specific vehicle - // type, as it is not appropriate for all - // types of vehicles (eg. fixed wing). + + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). if (scaled < 0.0F) { scaled = 0.0F; - } + } + if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); @@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); + } else { msg.cmd.push_back(static_cast(0)); } @@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm) { if (arm) { _armed_mask = -1; + } else { _armed_mask = 0; } @@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm) { if (arm) { _armed_mask = MOTOR_BIT(num); + } else { _armed_mask = 0; } @@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< } } -void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) { _esc_status.counter += 1; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; if (_esc_status_pub > 0) { (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c0355422..67bb47cc14 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -54,7 +54,7 @@ class UavcanEscController { public: - UavcanEscController(uavcan::INode& node); + UavcanEscController(uavcan::INode &node); ~UavcanEscController(); int init(); @@ -79,12 +79,12 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; - typedef uavcan::MethodBinder&)> StatusCbBinder; - typedef uavcan::MethodBinder - TimerCbBinder; + typedef uavcan::MethodBinder + TimerCbBinder; bool _armed = false; esc_status_s _esc_status = {}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1d6c205586..da62e55634 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ + uavcan_servers.cpp \ uavcan_clock.cpp \ uavcan_params.c @@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS +override EXTRADEFINES := $(EXTRADEFINES) \ +-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ +-DUAVCAN_NO_ASSERTIONS \ +-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \ +-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 3ae07367fa..52e4abbaf1 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -46,20 +46,21 @@ const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : -_node(node), -_sub_fix(node), -_report_pub(-1) + _node(node), + _sub_fix(node), + _report_pub(-1) { } int UavcanGnssBridge::init() { int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } + return res; } @@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const void UavcanGnssBridge::print_status() const { printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { printf("N/A\n"); + } else { printf("%d\n", _receiver_node_id); } @@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(pos_cov[8]) : -1.0F; + } else { report.eph = -1.0F; report.epv = -1.0F; } if (valid_velocity_covariance) { - float vel_cov[9]; - msg.velocity_covariance.unpackSquareMatrix(vel_cov); + float vel_cov[9]; + msg.velocity_covariance.unpackSquareMatrix(vel_cov); report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. @@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure(arg), sizeof(_scale)); - return 0; - } + std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); + return 0; + } + case MAGIOCGSCALE: { - std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); - return 0; - } + std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCSELFTEST: { - return 0; // Nothing to do - } + return 0; // Nothing to do + } + case MAGIOCGEXTERNAL: { - return 1; // declare it external rise it's priority and to allow for correct orientation compensation - } + return 1; // declare it external rise it's priority and to allow for correct orientation compensation + } + case MAGIOCSSAMPLERATE: { - return 0; // Pretend that this stuff is supported to keep the sensor app happy - } + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: case MAGIOCGSAMPLERATE: case MAGIOCSRANGE: @@ -121,15 +132,17 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar case MAGIOCSLOWPASS: case MAGIOCEXSTRAP: case MAGIOCGLOWPASS: { - return -EINVAL; - } + return -EINVAL; + } + default: { - return CDev::ioctl(filp, cmd, arg); - } + return CDev::ioctl(filp, cmd, arg); + } } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure + &msg) { lock(); _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index b370764440..e19235e525 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -45,7 +45,7 @@ /* * IUavcanSensorBridge */ -void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) +void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { list.add(new UavcanBarometerBridge(node)); list.add(new UavcanMagnetometerBridge(node)); @@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } + delete [] _channels; } @@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { _out_of_channels = true; log("out of class instances"); @@ -119,6 +121,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) channel->class_instance = class_instance; channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); + if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); @@ -128,6 +131,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } + assert(channel != nullptr); (void)orb_publish(_orb_topic, channel->orb_advert, report); @@ -136,11 +140,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { if (_channels[i].node_id >= 0) { out += 1; } } + return out; } @@ -152,6 +158,7 @@ void UavcanCDevSensorBridgeBase::print_status() const if (_channels[i].node_id >= 0) { printf("channel %d: node id %d --> class instance %d\n", i, _channels[i].node_id, _channels[i].class_instance); + } else { printf("channel %d: empty\n", i); } diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index de130b0788..dc2a1983b0 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -45,7 +45,7 @@ /** * A sensor bridge class must implement this interface. */ -class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode { public: static constexpr unsigned MAX_NAME_LEN = 20; @@ -77,7 +77,7 @@ public: * Sensor bridge factory. * Creates all known sensor bridges and puts them in the linked list. */ - static void make_all(uavcan::INode &node, List &list); + static void make_all(uavcan::INode &node, List &list); }; /** @@ -86,8 +86,7 @@ public: */ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev { - struct Channel - { + struct Channel { int node_id = -1; orb_advert_t orb_advert = -1; int class_instance = -1; @@ -104,13 +103,13 @@ protected: static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor, - const unsigned max_channels = DEFAULT_MAX_CHANNELS) : - device::CDev(name, devname), - _max_channels(max_channels), - _class_devname(class_devname), - _orb_topic(orb_topic_sensor), - _channels(new Channel[max_channels]) + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : + device::CDev(name, devname), + _max_channels(max_channels), + _class_devname(class_devname), + _orb_topic(orb_topic_sensor), + _channels(new Channel[max_channels]) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f4763fce7f..94261ac9cf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,15 +53,12 @@ #include #include "uavcan_main.hpp" +#include -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 -#endif + /** * @file uavcan_main.cpp @@ -76,33 +73,28 @@ * UavcanNode */ UavcanNode *UavcanNode::_instance; -#if defined(USE_FW_NODE_SERVER) -uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance; -uavcan_posix::dynamic_node_id_server::FileEventTracer tracer; -uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; -uavcan_posix::FirmwareVersionChecker fw_version_checker; -#endif UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), -#if !defined(USE_FW_NODE_SERVER) _esc_controller(_node) -#else - _esc_controller(_node), - _fileserver_backend(_node), - _node_info_retriever(_node), - _fw_upgrade_trigger(_node, fw_version_checker), - _fw_server(_node, _fileserver_backend) -#endif - { + _task_should_exit = false; + _fw_server_action = None; + _fw_server_status = -1; + _tx_injector = nullptr; _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - const int res = pthread_mutex_init(&_node_mutex, nullptr); + int res = pthread_mutex_init(&_node_mutex, nullptr); + + if (res < 0) { + std::abort(); + } + + res = sem_init(&_server_command_sem, 0 , 0); if (res < 0) { std::abort(); @@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys UavcanNode::~UavcanNode() { + + fw_server(Stop); + if (_task != -1) { + /* tell the task we want it to go away */ _task_should_exit = true; @@ -161,13 +157,127 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); - -#if defined(USE_FW_NODE_SERVER) - delete(_server_instance); -#endif + pthread_mutex_destroy(&_node_mutex); + sem_destroy(&_server_command_sem); } +int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) +{ + int rv = -1; + + if (UavcanNode::instance()) { + if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { + hwver.major = 1; + + } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { + hwver.major = 2; + + } else { + ; // All other values of HW_ARCH resolve to zero + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + rv = 0; + } + + return rv; +} + +int UavcanNode::start_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers == nullptr) { + + rv = UavcanServers::start(2, _node); + + if (rv >= 0) { + /* + * Set our pointer to to the injector + * This is a work around as + * main_node.getDispatcher().installRxFrameListener(driver.get()); + * would require a dynamic cast and rtti is not enabled. + */ + UavcanServers::instance()->attachITxQueueInjector(&_tx_injector); + } + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + +int UavcanNode::request_fw_check() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + _servers->requestCheckAllNodesFirmwareAndUpdate(); + rv = 0; + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; + +} + +int UavcanNode::stop_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + /* + * Set our 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. + */ + _tx_injector = nullptr; + + rv = _servers->stop(); + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + + +int UavcanNode::fw_server(eServerAction action) +{ + int rv = -EAGAIN; + + switch (action) { + case Start: + case Stop: + case CheckFW: + if (_fw_server_action == None) { + _fw_server_action = action; + sem_wait(&_server_command_sem); + rv = _fw_server_status; + } + + break; + + default: + rv = -EINVAL; + break; + } + + return rv; +} + + int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { @@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; + } + const int node_init_res = _instance->init(node_id); if (node_init_res < 0) { @@ -248,7 +363,7 @@ void UavcanNode::fill_node_info() assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); @@ -256,21 +371,7 @@ void UavcanNode::fill_node_info() /* hardware version */ uavcan::protocol::HardwareVersion hwver; - - if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) { - hwver.major = 1; - - } else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) { - hwver.major = 2; - - } else { - ; // All other values of HW_ARCH resolve to zero - } - - uint8_t udid[12] = {}; // Someone seems to love magic numbers - get_board_serial(udid); - uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); - + getHardwareVersion(hwver); _node.setHardwareVersion(hwver); } @@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id) br = br->getSibling(); } -#if defined(USE_FW_NODE_SERVER) - /* Initialize the fw version checker. - * giving it it's path - */ - ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); - - if (ret < 0) { - return ret; - } - - - /* Start fw file server back */ - - ret = _fw_server.start(); - - if (ret < 0) { - 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) { - return ret; - } - - /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ - - ret = tracer.init(UAVCAN_LOG_FILE); - - if (ret < 0) { - return ret; - } - - /* Create dynamic node id server for the Firmware updates directory */ - - _server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer); - - if (_server_instance == 0) { - return -ENOMEM; - } - - /* Initialize the dynamic node id server */ - ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id); - - if (ret < 0) { - return ret; - } - - /* Start node info retriever to fetch node info from new nodes */ - - ret = _node_info_retriever.start(); - - if (ret < 0) { - return ret; - } - - - /* Start the fw version checker */ - - ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath()); - - if (ret < 0) { - return ret; - } - -#endif /* Start the Node */ return _node.start(); @@ -398,6 +431,11 @@ void UavcanNode::node_spin_once() warnx("node spin error %i", spin_res); } + + if (_tx_injector != nullptr) { + _tx_injector->injectTxFramesInto(_node); + } + perf_end(_perfcnt_node_spin_elapsed); } @@ -446,7 +484,7 @@ int UavcanNode::run() * IO multiplexing shall be done here. */ - _node.setStatusOk(); + _node.setModeOperational(); /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). @@ -466,6 +504,24 @@ int UavcanNode::run() while (!_task_should_exit) { + switch (_fw_server_action) { + case Start: + _fw_server_status = start_fw_server(); + break; + + case Stop: + _fw_server_status = stop_fw_server(); + break; + + case CheckFW: + _fw_server_status = request_fw_check(); + break; + + case None: + default: + break; + } + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); @@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { + sem_post(&_server_command_sem); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); @@ -812,7 +870,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop|arm|disarm}"); + "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[]) ::exit(1); } + bool fw = argc > 2 && !std::strcmp(argv[2], "fw"); + if (!std::strcmp(argv[1], "start")) { if (UavcanNode::instance()) { + if (fw && UavcanServers::instance() == nullptr) { + int rv = UavcanNode::instance()->fw_server(UavcanNode::Start); + + if (rv < 0) { + warnx("Firmware Server Failed to Start %d", rv); + ::exit(rv); + } + + ::exit(0); + } + // Already running, no error warnx("already started"); ::exit(0); @@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[]) errx(1, "application not running"); } + if (fw && !std::strcmp(argv[1], "update")) { + if (UavcanServers::instance() == nullptr) { + errx(1, "firmware server is not running"); + } + + int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW); + ::exit(rv); + } + + if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) { + printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped"); + ::exit(0); + } + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); ::exit(0); @@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); + if (fw) { + + int rv = inst->fw_server(UavcanNode::Stop); + + if (rv < 0) { + warnx("Firmware Server Failed to Stop %d", rv); + ::exit(rv); + } + + ::exit(0); + + } else { + delete inst; + ::exit(0); + } } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 43d82082b4..3413f31bf1 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -48,13 +48,7 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include -# include -# include -#endif +# include "uavcan_servers.hpp" /** * @file uavcan_main.hpp @@ -66,9 +60,6 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #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" // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) @@ -84,8 +75,7 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 10; - static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why - + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -99,11 +89,12 @@ class UavcanNode : public device::CDev */ static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 3400; + static constexpr unsigned StackSize = 1600; public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; + enum eServerAction {None, Start, Stop, CheckFW , Busy}; UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); @@ -126,6 +117,9 @@ public: void print_info(); static UavcanNode *instance() { return _instance; } + static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); + int fw_server(eServerAction action); + void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} private: void fill_node_info(); @@ -133,10 +127,14 @@ private: void node_spin_once(); int run(); int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] - + int start_fw_server(); + int stop_fw_server(); + int request_fw_check(); int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + volatile eServerAction _fw_server_action; + int _fw_server_status; int _armed_sub = -1; ///< uORB subscription of the arming status actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus @@ -151,22 +149,13 @@ private: Node _node; ///< library instance pthread_mutex_t _node_mutex; - + sem_t _server_command_sem; UavcanEscController _esc_controller; - -#if defined(USE_FW_NODE_SERVER) - static 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; -#endif List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; - + ITxQueueInjector *_tx_injector; uint32_t _groups_required = 0; uint32_t _groups_subscribed = 0; int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp new file mode 100644 index 0000000000..6c4d70e3cd --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -0,0 +1,280 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uavcan_main.hpp" +#include "uavcan_servers.hpp" +#include "uavcan_virtual_can_driver.hpp" + +# include +# include +# include + +//todo:The Inclusion of file_server_backend is killing +// #include and leaving OK undefined +# define OK 0 + +/** + * @file uavcan_servers.cpp + * + * Implements basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * David Sidrane + */ + +/* + * 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); + } +} + +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 %i", rv); + delete _instance; + 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(run_trampoline), NULL); + + if (rv < 0) { + warnx("start failed: %d", errno); + rv = -errno; + delete _instance; + } + + return rv; +} + +int UavcanServers::init(unsigned num_ifaces) +{ + /* + * Initialize the mutex. + * giving it its path + */ + + int ret = Lock::init(_subnode_mutex); + + if (ret < 0) { + 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) { + return ret; + } + + /* Start fw file server back */ + + ret = _fw_server.start(); + + if (ret < 0) { + 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) { + return ret; + } + + /* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ + + ret = _tracer.init(UAVCAN_LOG_FILE); + + if (ret < 0) { + 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) { + return ret; + } + + /* Start node info retriever to fetch node info from new nodes */ + + ret = _node_info_retriever.start(); + + if (ret < 0) { + return ret; + } + + /* Start the fw version checker */ + + ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath()); + + if (ret < 0) { + 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; +} + diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp new file mode 100644 index 0000000000..9a68a8cc25 --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -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 + +#include +#include +#include + +#include +#include + +# include +# include +# include +# include +# include +# include +# include +# include + +# include "uavcan_virtual_can_driver.hpp" + +/** + * @file uavcan_servers.hpp + * + * Defines basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * @author David Sidrane + */ + +#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 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 vCanDriver; + + vCanDriver _vdriver; + + uavcan::SubNode _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; + +}; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp new file mode 100644 index 0000000000..7a151d2cd2 --- /dev/null +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -0,0 +1,506 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Pavel Kirienko + * David Sidrane + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +/* + * 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 +class Queue +{ + struct Item : public uavcan::LinkedListNode { + T payload; + + template + Item(Args... args) : payload(args...) { } + }; + + uavcan::LimitedPoolAllocator allocator_; + uavcan::LinkedListRoot list_; + +public: + Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) : + allocator_(arg_allocator, block_allocation_quota) + { + uavcan::IsDynamicallyAllocatable::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 + 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 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(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 +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 allocator_; ///< Shared across all ifaces + uavcan::LazyConstructor 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(allocator_, clock_, driver_mutex_, quota_per_queue); + } + } + + ~VirtualCanDriver() + { + Lock::deinit(driver_mutex_); + event_.deinit(); + } + +};