diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 1dd0a8c67c..d2f4a00e7a 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -45,6 +45,7 @@
 
 #include <cstdlib>
 #include <cstring>
+#include <array>
 #include <fcntl.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -84,6 +85,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_hardpoint_controller(_node),
 	_time_sync_master(_node),
 	_time_sync_slave(_node),
+	_node_status_monitor(_node),
 	_master_timer(_node),
 	_setget_response(0)
 
@@ -806,7 +808,7 @@ int UavcanNode::run()
 	_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
 	_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
 
-
+	_node_status_monitor.start();
 
 	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
 
@@ -1253,6 +1255,18 @@ UavcanNode::print_info()
 		br = br->getSibling();
 	}
 
+	// Printing all nodes that are online
+	std::printf("Online nodes (Node ID, Health, Mode):\n");
+	_node_status_monitor.forEachNode([](uavcan::NodeID nid, uavcan::NodeStatusMonitor::NodeStatus ns) {
+		static constexpr std::array<const char*, 4> HEALTH {
+			"OK", "WARN", "ERR", "CRIT"
+		};
+		static constexpr std::array<const char*, 8> MODES {
+			"OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN"
+		};
+		std::printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH.at(ns.health), MODES.at(ns.mode));
+	});
+
 	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index dbb74b39af..b962333a06 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -48,6 +48,7 @@
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
 #include <uavcan/protocol/global_time_sync_master.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
 #include <uavcan/protocol/param/GetSet.hpp>
 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
 #include <uavcan/protocol/RestartNode.hpp>
@@ -140,8 +141,6 @@ public:
 	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);
@@ -187,6 +186,7 @@ private:
 	UavcanHardpointController	_hardpoint_controller;
 	uavcan::GlobalTimeSyncMaster	_time_sync_master;
 	uavcan::GlobalTimeSyncSlave	_time_sync_slave;
+	uavcan::NodeStatusMonitor	_node_status_monitor;
 
 	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges