diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fdc23a889f..a04854832a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -66,6 +66,7 @@ set(msg_files DifferentialPressure.msg DistanceSensor.msg DistanceSensorModeChangeRequest.msg + DronecanNodeStatus.msg Ekf2Timestamps.msg EscReport.msg EscStatus.msg diff --git a/msg/DronecanNodeStatus.msg b/msg/DronecanNodeStatus.msg new file mode 100644 index 0000000000..7751f9f076 --- /dev/null +++ b/msg/DronecanNodeStatus.msg @@ -0,0 +1,41 @@ +uint64 timestamp # time since system start (microseconds) + +uint16 node_id # The node ID which this data comes from + +# From the uavcan.protocol.NodeStatus message +uint32 uptime_sec # Node uptime + +# +# Abstract node health. +# +uint8 HEALTH_OK = 0 # The node is functioning properly. +uint8 HEALTH_WARNING = 1 # A critical parameter went out of range or the node encountered a minor failure. +uint8 HEALTH_ERROR = 2 # The node encountered a major failure. +uint8 HEALTH_CRITICAL = 3 # The node suffered a fatal malfunction. +uint8 health + +# +# Current mode. +# +# Mode OFFLINE can be actually reported by the node to explicitly inform other network +# participants that the sending node is about to shutdown. In this case other nodes will not +# have to wait OFFLINE_TIMEOUT_MS before they detect that the node is no longer available. +# +# Reserved values can be used in future revisions of the specification. +# +uint8 MODE_OPERATIONAL = 0 # Normal operating mode. +uint8 MODE_INITIALIZATION = 1 # Initialization is in progress; this mode is entered immediately after startup. +uint8 MODE_MAINTENANCE = 2 # E.g. calibration, the bootloader is running, etc. +uint8 MODE_SOFTWARE_UPDATE = 3 # New software/firmware is being loaded. +uint8 MODE_OFFLINE = 7 # The node is no longer available. +uint8 mode + +# +# Not used currently, keep zero when publishing, ignore when receiving. +# +uint8 sub_mode + +# +# Optional, vendor-specific node status code, e.g. a fault code or a status bitmask. +# +uint16 vendor_specific_status_code diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index c915ec6d10..942f67c148 100644 --- a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -24,14 +24,18 @@ class UAVCAN_EXPORT NodeStatusMonitor public: struct NodeStatus { + uint32_t uptime_sec; uint8_t health : 2; uint8_t mode : 3; uint8_t sub_mode : 3; + uint16_t vendor_specific_status_code; NodeStatus() : + uptime_sec(0), health(protocol::NodeStatus::HEALTH_CRITICAL), mode(protocol::NodeStatus::MODE_OFFLINE), - sub_mode(0) + sub_mode(0), + vendor_specific_status_code(0) { StaticAssert::check(); StaticAssert::check(); @@ -123,9 +127,11 @@ private: { Entry new_entry; new_entry.time_since_last_update_ms100 = 0; + new_entry.status.uptime_sec = msg.uptime_sec; new_entry.status.health = msg.health & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1); new_entry.status.mode = msg.mode & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1); new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1); + new_entry.status.vendor_specific_status_code = msg.vendor_specific_status_code; changeNodeStatus(msg.getSrcNodeID(), new_entry); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 7fb665a87c..c3e3ebe5c7 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -746,40 +746,9 @@ UavcanNode::Run() _node.spinOnce(); // expected to be non-blocking - // Publish status - constexpr hrt_abstime status_pub_interval = 100_ms; + publish_can_interface_statuses(); - if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) { - _last_can_status_pub = hrt_absolute_time(); - - for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { - if (i > UAVCAN_NUM_IFACES) { - break; - } - - auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); - - if (!iface) { - continue; - } - - auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); - can_interface_status_s status{ - .timestamp = hrt_absolute_time(), - .io_errors = iface_perf_cnt.errors, - .frames_tx = iface_perf_cnt.frames_tx, - .frames_rx = iface_perf_cnt.frames_rx, - .interface = static_cast(i), - }; - - if (_can_status_pub_handles[i] == nullptr) { - int instance{0}; - _can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance); - } - - (void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status); - } - } + publish_node_statuses(); // check for parameter updates if (_parameter_update_sub.updated()) { @@ -1011,6 +980,101 @@ UavcanNode::Run() } } +void UavcanNode::publish_can_interface_statuses() +{ + constexpr hrt_abstime status_pub_interval = 100_ms; + + if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) { + _last_can_status_pub = hrt_absolute_time(); + + for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + if (i > UAVCAN_NUM_IFACES) { + break; + } + + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); + + if (!iface) { + continue; + } + + auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); + can_interface_status_s status{ + .timestamp = hrt_absolute_time(), + .io_errors = iface_perf_cnt.errors, + .frames_tx = iface_perf_cnt.frames_tx, + .frames_rx = iface_perf_cnt.frames_rx, + .interface = static_cast(i), + }; + + if (_can_status_pub_handles[i] == nullptr) { + int instance{0}; + _can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance); + } + + (void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status); + } + } +} + +void UavcanNode::publish_node_statuses() +{ + constexpr hrt_abstime status_pub_interval = 100_ms; + + if (hrt_absolute_time() - _last_node_status_pub >= status_pub_interval) { + _last_node_status_pub = hrt_absolute_time(); + + _node_status_monitor.forEachNode([this](uavcan::NodeID node_id, uavcan::NodeStatusMonitor::NodeStatus node_status) { + + if (node_id.get() == 0) { + return; + } + + // See if we have NodeID <--> uORB_index mapped + int uorb_index = -1; + + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_node_status_uorb_index_map[i] == node_id.get()) { + uorb_index = i; + break; + } + } + + if (uorb_index < 0) { + // use next available index + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_node_status_uorb_index_map[i] == 0) { + _node_status_uorb_index_map[i] = node_id.get(); + uorb_index = i; + // advertise + PX4_INFO("advertising node_id %u on index %u", node_id.get(), i); + int instance{0}; + _node_status_pub_handles[i] = orb_advertise_multi(ORB_ID(dronecan_node_status), nullptr, &instance); + break; + } + } + } + + if (uorb_index >= 0) { + + dronecan_node_status_s status{ + .timestamp = hrt_absolute_time(), + .uptime_sec = node_status.uptime_sec, + .node_id = node_id.get(), + .vendor_specific_status_code = node_status.vendor_specific_status_code, + .health = node_status.health, + .mode = node_status.mode, + .sub_mode = node_status.sub_mode, + }; + + (void)orb_publish(ORB_ID(dronecan_node_status), _node_status_pub_handles[uorb_index], &status); + } + }); + + + } +} + #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 15fb23e413..8af4ae1a3e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -101,6 +101,7 @@ #include #include #include +#include #include #include #include @@ -235,6 +236,9 @@ private: void fill_node_info(); int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events); + void publish_can_interface_statuses(); + void publish_node_statuses(); + 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 update_params(); @@ -317,10 +321,15 @@ private: uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; - uORB::PublicationMulti _can_status_pub{ORB_ID(can_interface_status)}; + + orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr}; + + // array of NodeIDs, each index maps to uORB index + orb_advert_t _node_status_pub_handles[ORB_MULTI_MAX_INSTANCES] = {nullptr}; + uint8_t _node_status_uorb_index_map[ORB_MULTI_MAX_INSTANCES] = {}; hrt_abstime _last_can_status_pub{0}; - orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr}; + hrt_abstime _last_node_status_pub{0}; /* * The MAVLink parameter bridge needs to know the maximum parameter index diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 597b4abf4e..8633cd8d2e 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -59,6 +59,7 @@ void LoggedTopics::add_default_topics() add_topic("config_overrides"); add_topic("cpuload"); add_topic("distance_sensor_mode_change_request"); + add_topic_multi("dronecan_node_status", 250); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position");